#include "arm_behaviour.h"
// #include "arm_math.h"
// #include "bsp_buzzer.h"
// #include "detect_task.h"

#include "user_lib.h"

#define int_abs(x) ((x) > 0 ? (x) : (-x))
/**
  * @brief          遥控器的死区判断，
  * @param          输入的遥控器值
  * @param          输出的死区处理后遥控器值
  * @param          死区值
  */
#define rc_deadband_limit(input, output, dealine)        \
    {                                                    \
        if ((input) > (dealine) || (input) < -(dealine)) \
        {                                                \
            (output) = (input);                          \
        }                                                \
        else                                             \
        {                                                \
            (output) = 0;                                \
        }                                                \
    }



/**
  * @brief          机械臂行为状态机设置.
  * @param[in]      arm_mode_set: 机械臂数据指针
  * @retval         none
  */
static void arm_behavour_set(arm_control_t *arm_mode_set);

/**
  * @brief          当机械臂行为模式是ARM_ZERO_FORCE, 这个函数会被调用,机械臂控制模式是raw模式.原始模式意味着
  *                 设定值会直接发送到CAN总线上,这个函数将会设置所有为0.
  * @param[in]      jointx 为各关节的控制角度 单位 rad
  * @param[in]      arm_control_set: 机械臂数据指针
  * @retval         none
  */
static void arm_zero_force_control(fp32 *joint1, fp32 *joint2, fp32 *joint3, fp32 *joint4, fp32 *motor5, fp32 *motor6, arm_control_t *arm_control_set);

/**
  * @brief         机械臂角度初始化，先让关节2和3初始化，机械臂抬起来，扩大机械臂活动范围。然后再让其它关节初始化起来
  * @param[in]      jointx 为各关节的控制角度 单位 rad
  * @param[in]      机械臂数据指针
  * @retval         返回空
  */
static void arm_init_control(fp32 *joint1, fp32 *joint2, fp32 *joint3, fp32 *joint4, fp32 *motor5, fp32 *motor6, arm_control_t *arm_control_set);

/**
  * @brief          机械臂编码值控制，电机是相对角度控制，
  * @param[in]      jointx 为各关节的控制角度 单位 rad
  * @param[in]      arm_control_set: 机械臂数据指针
  * @retval         none
  */
static void arm_relative_angle_control(fp32 *joint1, fp32 *joint2, fp32 *joint3, fp32 *joint4, fp32 *motor5, fp32 *motor6, arm_control_t *arm_control_set);

/**
  * @brief          机械臂进入遥控器无输入控制，电机是相对角度控制，暂时舍弃未定义
  * @author         RM
  * @param[in]      jointx 为各关节的控制角度 单位 rad
  * @param[in]      arm_control_set:机械臂数据指针
  * @retval         none
  */
// static void arm_motionless_control(fp32 *joint1, fp32 *joint2, fp32 *joint3, fp32 *joint4, fp32 *motor5, fp32 *motor6, arm_control_t *arm_control_set);

//机械臂行为状态机
static arm_behaviour_e arm_behaviour = ARM_RELATIVE_ANGLE;


// void arm_behaviour_control_set(fp32 *add_joint1, fp32 *add_joint2, fp32 *add_joint3, fp32 *add_joint4, fp32 *add_motor5, fp32 *add_motor6, arm_control_t *arm_control_set)
// bool_t arm_cmd_to_cup_stop(void)

/**
  * @brief          被arm_set_mode函数调用在arm_task.c,机械臂行为状态机以及电机状态机设置
  * @param[out]     arm_mode_set: 机械臂数据指针
  * @retval         none
  */

void arm_behaviour_mode_set(arm_control_t *arm_mode_set)
{
    if (arm_mode_set == NULL)
    {
        return;
    }
    //set arm_behaviour variable
    //机械臂行为状态机设置
    arm_behavour_set(arm_mode_set);

    //accoring to arm_behaviour, set motor control mode
    //根据机械臂行为状态机设置电机状态机
    if (arm_behaviour == ARM_ZERO_FORCE)
    {
        arm_mode_set->arm_joint1_motor.arm_motor_mode = ARM_MOTOR_RAW;
        arm_mode_set->arm_joint2_motor.arm_motor_mode = ARM_MOTOR_RAW;
        arm_mode_set->arm_joint3_motor.arm_motor_mode = ARM_MOTOR_RAW;
        arm_mode_set->arm_joint4_motor.arm_motor_mode = ARM_MOTOR_RAW;
        arm_mode_set->arm_motor5_motor.arm_motor_mode = ARM_MOTOR_RAW;
        arm_mode_set->arm_motor6_motor.arm_motor_mode = ARM_MOTOR_RAW;
    }
    else if (arm_behaviour == ARM_INIT)
    {
        arm_mode_set->arm_joint1_motor.arm_motor_mode = ARM_MOTOR_ENCONDE;
        arm_mode_set->arm_joint2_motor.arm_motor_mode = ARM_MOTOR_ENCONDE;
        arm_mode_set->arm_joint3_motor.arm_motor_mode = ARM_MOTOR_ENCONDE;
        arm_mode_set->arm_joint4_motor.arm_motor_mode = ARM_MOTOR_ENCONDE;
        arm_mode_set->arm_motor5_motor.arm_motor_mode = ARM_MOTOR_ENCONDE;
        arm_mode_set->arm_motor6_motor.arm_motor_mode = ARM_MOTOR_ENCONDE;
    }
    else if (arm_behaviour == ARM_RELATIVE_ANGLE)
    {
        arm_mode_set->arm_joint1_motor.arm_motor_mode = ARM_MOTOR_ENCONDE;
        arm_mode_set->arm_joint2_motor.arm_motor_mode = ARM_MOTOR_ENCONDE;
        arm_mode_set->arm_joint3_motor.arm_motor_mode = ARM_MOTOR_ENCONDE;
        arm_mode_set->arm_joint4_motor.arm_motor_mode = ARM_MOTOR_ENCONDE;
        arm_mode_set->arm_motor5_motor.arm_motor_mode = ARM_MOTOR_ENCONDE;
        arm_mode_set->arm_motor6_motor.arm_motor_mode = ARM_MOTOR_ENCONDE;
    }
}

/**
  * @brief          机械臂行为状态机设置.
  * @param[in]      arm_mode_set: 机械臂数据指针
  * @retval         none
  */
static void arm_behavour_set(arm_control_t *arm_mode_set)
{
    if (arm_mode_set == NULL)
    {
        return;
    }

    //init mode, judge if ARM is in middle place
    //初始化模式判断是否到达机器臂初始角度位置
    if (arm_behaviour == ARM_INIT)
    {
        static uint16_t init_time = 0;
        static uint16_t init_stop_time = 0;
        init_time++;
        
        if ((fabs(arm_mode_set->arm_joint1_motor.relative_angle - INIT_JOINT1_SET) < ARM_INIT_ANGLE_ERROR &&
             fabs(arm_mode_set->arm_joint2_motor.relative_angle - INIT_JOINT2_SET) < ARM_INIT_ANGLE_ERROR &&
             fabs(arm_mode_set->arm_joint3_motor.relative_angle - INIT_JOINT3_SET) < ARM_INIT_ANGLE_ERROR &&
             fabs(arm_mode_set->arm_joint4_motor.relative_angle - INIT_JOINT4_SET) < ARM_INIT_ANGLE_ERROR &&
             fabs(arm_mode_set->arm_motor5_motor.relative_angle - INIT_motor5_SET) < ARM_INIT_ANGLE_ERROR &&
             fabs(arm_mode_set->arm_motor6_motor.relative_angle - INIT_motor6_SET) < ARM_INIT_ANGLE_ERROR ))
        {
            
            if (init_stop_time < ARM_INIT_STOP_TIME)
            {
                init_stop_time++;
            }
        }
        else
        {
            
            if (init_time < ARM_INIT_TIME)
            {
                init_time++;
            }
        }

        //超过初始化最大时间，或者已经稳定到中值一段时间，退出初始化状态开关打下档，或者掉线
        if (init_time < ARM_INIT_TIME && init_stop_time < ARM_INIT_STOP_TIME &&
            !switch_is_down(arm_mode_set->arm_rc_ctrl->rc.s[ARM_MODE_CHANNEL]))// && !toe_is_error(DBUS_TOE)
        {
            return;
        }
        else
        {
            init_stop_time = 0;
            init_time = 0;
        }
    }

    //开关控制 机械臂状态   借用RM遥控器的开关通道设置机械臂状态，后续自定义遥控器出来了根据实际情况跟换或注释掉
    if (switch_is_down(arm_mode_set->arm_rc_ctrl->rc.s[ARM_MODE_CHANNEL]))
    {
        arm_behaviour = ARM_ZERO_FORCE;
    }
    else if (switch_is_mid(arm_mode_set->arm_rc_ctrl->rc.s[ARM_MODE_CHANNEL]))
    {
        arm_behaviour = ARM_RELATIVE_ANGLE;
    }

    // if( toe_is_error(DBUS_TOE))
    // {
    //     arm_behaviour = ARM_ZERO_FORCE;
    // }

    //enter init mode
    //判断进入init状态机
    {
        static arm_behaviour_e last_arm_behaviour = ARM_ZERO_FORCE;
        if (last_arm_behaviour == ARM_ZERO_FORCE && arm_behaviour != ARM_ZERO_FORCE)
        {
            arm_behaviour = ARM_INIT;
        }
        last_arm_behaviour = arm_behaviour;
    }

}

/**
  * @brief          机械臂行为控制，根据不同行为采用不同控制函数获取机械臂关节的控制角度
  * @param[in]      jointx 为各关节的控制角度 单位 rad
  * @param[in]      arm_mode_set:机械臂数据指针
  * @retval         none
  */
void arm_behaviour_control_set(fp32 *joint1, fp32 *joint2, fp32 *joint3, fp32 *joint4, fp32 *motor5, fp32 *motor6, arm_control_t *arm_control_set)
{

    if (joint1 == NULL || joint2 == NULL ||joint3 == NULL ||joint4 == NULL ||motor5 == NULL ||motor6 == NULL || arm_control_set == NULL)
    {
        return;
    }
    if (arm_behaviour == ARM_ZERO_FORCE)
    {
        arm_zero_force_control(joint1, joint2, joint3, joint4, motor5, motor6, arm_control_set);
    }
    else if (arm_behaviour == ARM_INIT)
    {
        arm_init_control(joint1,joint2,joint3,joint4,motor5,motor6,arm_control_set);

    }
    else if (arm_behaviour == ARM_RELATIVE_ANGLE)
    {
        arm_relative_angle_control(joint1, joint2, joint3, joint4, motor5, motor6, arm_control_set);

    }

}

/**
  * @brief          机械臂在某些行为下，需要吸盘停止
  * @param[in]      none
  * @retval         1: no move 0:normal
  */

bool_t arm_cmd_to_cup_stop(void)
{
    if (arm_behaviour == ARM_INIT  || arm_behaviour == ARM_ZERO_FORCE)
    {
        return 1;
    }
    else
    {
        return 0;
    }
}

/**
  * @brief          当机械臂行为模式是ARM_ZERO_FORCE,控制输出量为0 这个函数会被调用,机械臂控制模式是raw模式.原始模式意味着
  *                 设定值会直接发送到CAN总线上,这个函数将会设置所有为0.
  * @param[in]      jointx 为各关节的控制角度 单位 rad
  * @param[in]      arm_control_set: 机械臂数据指针
  * @retval         none
  */
static void arm_zero_force_control(fp32 *joint1, fp32 *joint2, fp32 *joint3, fp32 *joint4, fp32 *motor5, fp32 *motor6, arm_control_t *arm_control_set)
{
    if (joint1 == NULL || joint2 == NULL ||joint3 == NULL ||joint4 == NULL ||motor5 == NULL ||motor6 == NULL || arm_control_set == NULL)
    {
        return;
    }

    *joint1 = 0.0f;
    *joint2 = 0.0f;
    *joint3 = 0.0f;
    *joint4 = 0.0f;
    *motor5 = 0.0f;
    *motor6 = 0.0f;
}

/**
  * @brief          机械臂角度初始化，先让关节2和3初始化，机械臂抬起来，扩大机械臂活动范围。然后再让其它关节初始化起来
  * @author         RM
  * @param[in]      jointx 为各关节的控制角度 单位 rad
  * @param[in]      机械臂数据指针
  * @retval         返回空
  */
static void arm_init_control(fp32 *joint1, fp32 *joint2, fp32 *joint3, fp32 *joint4, fp32 *motor5, fp32 *motor6, arm_control_t *arm_control_set)
{
    if (joint1 == NULL || joint2 == NULL ||joint3 == NULL ||joint4 == NULL ||motor5 == NULL ||motor6 == NULL || arm_control_set == NULL)
    {
        return;
    }

    //初始化状态控制量计算，机械臂角度初始化，先让关节2和3初始化，机械臂抬起来，扩大机械臂活动范围。然后再让其它关节初始化起来
    if(arm_control_set->arm_rc_ctrl->joint.arm_init_mode==1)//值为1时初始化为模型控制角度（小机械臂的初始角度），为0时初始化为设置角度（定义的初始化角度）。
    {   
        if (fabs(arm_control_set->arm_rc_ctrl->joint.joint2_rad - arm_control_set->arm_joint2_motor.relative_angle) > ARM_INIT_ANGLE_ERROR||fabs(arm_control_set->arm_rc_ctrl->joint.joint3_rad - arm_control_set->arm_joint3_motor.relative_angle) > ARM_INIT_ANGLE_ERROR)
        {
        *joint2 = arm_control_set->arm_rc_ctrl->joint.joint2_rad;
        *joint3 = arm_control_set->arm_rc_ctrl->joint.joint3_rad;
        *joint1 = 0.0f;
        *joint4 = 0.0f;
        *motor5 = 0.0f;
        *motor6 = 0.0f;
        }
        else
        {
        *joint2 = arm_control_set->arm_rc_ctrl->joint.joint2_rad;
        *joint3 = arm_control_set->arm_rc_ctrl->joint.joint3_rad;
        *joint1 = arm_control_set->arm_rc_ctrl->joint.joint1_rad;
        *joint4 = arm_control_set->arm_rc_ctrl->joint.joint4_rad;
        *motor5 = arm_control_set->arm_rc_ctrl->joint.joint5_rad;
        *motor6 = arm_control_set->arm_rc_ctrl->joint.joint6_rad;
        }
    }
    else
    {
        if (fabs(INIT_JOINT2_SET - arm_control_set->arm_joint2_motor.relative_angle) > ARM_INIT_ANGLE_ERROR||fabs(INIT_JOINT3_SET - arm_control_set->arm_joint3_motor.relative_angle) > ARM_INIT_ANGLE_ERROR)
        {
        *joint2 = INIT_JOINT2_SET;
        *joint3 = INIT_JOINT3_SET;
        *joint1 = 0.0f;
        *joint4 = 0.0f;
        *motor5 = 0.0f;
        *motor6 = 0.0f;
        }
        else
        {
        *joint2 = INIT_JOINT2_SET;
        *joint3 = INIT_JOINT3_SET;
        *joint1 = INIT_JOINT1_SET;
        *joint4 = INIT_JOINT4_SET;
        *motor5 = INIT_motor5_SET;
        *motor6 = INIT_motor6_SET;
        }
    }

}

/**
  * @brief          机械臂编码值控制，电机是相对角度控制，
  * @param[in]      jointx 为各关节的控制角度 单位 rad
  * @param[in]      arm_control_set: 机械臂数据指针
  * @retval         none
  */
static void arm_relative_angle_control(fp32 *joint1, fp32 *joint2, fp32 *joint3, fp32 *joint4, fp32 *motor5, fp32 *motor6, arm_control_t *arm_control_set)
{
    if (joint1 == NULL || joint2 == NULL ||joint3 == NULL ||joint4 == NULL ||motor5 == NULL ||motor6 == NULL || arm_control_set == NULL)
    {
        return;
    }
        //在这处理关节5，关节6与电机控制量的关系。以及2，3关节的解耦操作。

        *joint1 = arm_control_set->arm_rc_ctrl->joint.joint1_rad;
        *joint2 = arm_control_set->arm_rc_ctrl->joint.joint2_rad;
        //以及2，3关节的解耦,关节2旋转时，3反转相同角度
        *joint3 = arm_control_set->arm_rc_ctrl->joint.joint3_rad + arm_control_set->arm_rc_ctrl->joint.joint2_rad;
        *joint4 = arm_control_set->arm_rc_ctrl->joint.joint4_rad;
        //关节5，关节6与电机控制量的关系,关节6为最后的旋转轴
        *motor5 = -arm_control_set->arm_rc_ctrl->joint.joint5_rad*JOINT5_TO_MOTOR5_6_SCALE 
                                        + arm_control_set->arm_rc_ctrl->joint.joint6_rad*JOINT6_TO_MOTOR5_6_SCALE;
        *motor6 = arm_control_set->arm_rc_ctrl->joint.joint5_rad*JOINT5_TO_MOTOR5_6_SCALE
                                        + arm_control_set->arm_rc_ctrl->joint.joint6_rad*JOINT6_TO_MOTOR5_6_SCALE;
}





